#include "LaneChanging.h"
#include "TrajectoryPool.h"
#include <vector>

using namespace std;

LaneChanging::LaneChanging(int nStartLane, int nTargetLane)
	: m_nStartLane(nStartLane)
	, m_nTargetLane(nTargetLane) {}

tuple<VehicleState*, TrajectoryPtr> LaneChanging::OptimalTrajectory(const Eigen::VectorXd& currStateV6,
											double currTime,
											const SensorFusion& rSF)
{
	const int maxT = 10;
	const double currentD = currStateV6(3);
	const double startD = Utils::LaneNumberToD(m_nStartLane);
	const double targetD = Utils::LaneNumberToD(m_nTargetLane);
	const double changingLaneTime = 4;
	const int nCurrLane = Utils::DtoLaneNumber(currStateV6(3));
	const double MaxVelosityCorrected = GetCorrectedVelocity(m_SpeedLimit, nCurrLane);
	TrajectoryPool pool(MaxVelosityCorrected, m_HorizontPrediction);	
	pool.SetOtherCars(rSF.GetLeadingCarsTrajectoryInLane(currStateV6, m_nStartLane, 
														 m_HorizontPrediction, m_TimeStep));
	pool.AddOtherCars(rSF.GetOtherCarsTrajectoryInLane(currStateV6, m_nTargetLane, 
													   m_HorizontPrediction, m_TimeStep));
	for (double v = 0; v < m_SpeedLimit; v += 1)
	{
		for (double T = 1; T < maxT; T += 1)
		{
			pool.AddTrajectory(Trajectory::Generate_longitudinal_and_lateral_trajectory(currStateV6,
															 targetD, v, currTime, T, changingLaneTime));
		}
	}

	VehicleState* pNextState = this;
	TrajectoryPtr pOptimalTraj = pool.OptimalTrajectory();
	
	// 当assert的表达式（即参数expression）等于0或者为false时，assert会向stderr输出一些错误信息，并且调用abort终止程序运行
	assert(targetD == pOptimalTraj->GetTargetD());
	return tuple<VehicleState*, TrajectoryPtr>(pNextState, pOptimalTraj);
}
